/*
 * target.cpp
 *
 *  Created on: 2020年12月2日
 *      Author: Huanyu Zhang
 */

#include "target.hpp"

//TARGET target;

//void TARGET::Target_Command_Reset(void)
//{
//	xQueuePeek(queueTargetCommand, &target_command, 0);
//	for(int i =0 ; i< MAX_COMMAND_SIZE; ++i){
//		for(int j =0 ; j!=3 ; ++j){
//		 target_command.TargetPointList[i][j] = 0.0f; //NED坐标
//		}
//		target_command.TargetPointList[i][3] = -1.0f;
//	}
//	target_command.count = 0;
//	target_command.isUpdate = false;
//	target_command.index = 0;
//	xQueueOverwrite(queueTargetCommand,&target_command);
//}
//
//void TARGET::Target_Queue_Reset(void)
//{
//	target_queue.index = 0;
//	for(int i =0 ; i!=MAX_QUEUE_SIZE; ++i){
//	for(int j =0 ; j!=3 ; ++j)
//		target_queue.queue[i][j] = 0; //queue是一个20 * 4的矩阵，最后一列为-1，其他为0
//	target_queue.queue[i][3] = -1 ;
//	}
//}
//
//bool TARGET::Target_Queue_Push_Target(void)
//{	//将target_command里面的所有点都存入target_queue中
//	xQueuePeek(queueTargetCommand, &target_command, 0);
//	bool brake_flag = true;
//	for( int i =0 ; target_command.count >0 && target_queue.index != MAX_QUEUE_SIZE; ++i){  //退出的条件：要不就是队列已满，要不就是目标点已经完全加入至队列中
//		for(int j=0; j!=4;++j)
//			target_queue.queue[target_queue.index][j]=target_command.TargetPointList[target_command.index - target_command.count][j];
//		target_command.count--;
//		target_queue.index++;
//		if(-2 == target_queue.queue[i][3])
//		{	//遇到-2 == target_queue.queue[i][3]，说明在上位机上已经下达了“清除目标队列”，那么需要将队列中所有的点清除
//			Target_Queue_Reset(); //清除目标队列时总是先让坐标点为0(前面三个点),最后一个点为-1(悬停时间)
//			brake_flag = false;
//			break;
//		}
//	}
//	Target_Command_Reset(); //当把目标点加入至队列后，清除搬运数据的target数组，防止本次数据对下次数据有影响
//	xQueueOverwrite(queueTargetCommand,&target_command);
//	return brake_flag; //用返回值判断是否进行刹车操作
//}

void TARGET::Target_Reset(void){
	xQueuePeek(queueTargetPoint, &targetPoint, 0);
	for(int i = 0; i < MAX_QUEUE_SIZE; ++i){
		for(int j = 0; j < 3; ++j){
			targetPoint.TargetPointList[i][j] = 0.0f;
		}
		targetPoint.TargetPointList[i][3] = -1.0f;
		targetPoint.yawCommandList[i] = 0.0f;
	}
	targetPoint.fptr = 1;
	targetPoint.eptr = 1;
	targetPoint.isUpdate = false;
	xQueueOverwrite(queueTargetPoint,&targetPoint);
}

bool TARGET::Pop_Next_Target(float * target_p,float* hover_time)
{
	//检查队列是否为空
	if(Target_Queue_Is_Queue_Empty() == true) return false;

//	//检查下一个点的合法性，如果不合法，直接刹车
//	if(targetPoint.TargetPointList[targetPoint.fptr][3] == -1.0){
//		// brake_flag 标志位，暂时未写
//		/*
//		 *  待补充
//		 *
//		 * */
//		 return false;
//	}
	xQueuePeek(queueTargetPoint, &targetPoint, 0);
	for(int i = 0; i < 3; ++i){
		*(target_p+i) = targetPoint.TargetPointList[targetPoint.fptr][i];
	}
	*hover_time = targetPoint.TargetPointList[targetPoint.fptr][3] / 0.01; //目标队列中前三个数是坐标，最后一个是悬停时间，悬停时间为-1时相当于没有目标点
	targetPoint.fptr++;
	xQueueOverwrite(queueTargetPoint, &targetPoint);
	return true;
}

bool TARGET::Target_Queue_Is_Queue_Empty(void){
	//xQueuePeek(queueTargetPoint, &targetPoint, 0);
	return targetPoint.fptr == targetPoint.eptr ? true : false;
}

